program ptc_geometry
use madx_ptc_module
use pointer_lattice
use c_TPSA
implicit none

character*48 :: command_gino
type(probe) xs0,xs1,XST
type(probe_8) xs
type(layout), pointer :: als
real(dp) closed(6),theta0,XN(LNV),N0(3)  
type(real_8)  theta
type(damap) id,one_turn,a
type(normalform) nf
INTEGER MF,I,N,k,pos
TYPE(FIBRE),POINTER:: P
type(internal_state) state
type(damapspin) id_s,a_S,A_f,a_l,a_nl,dr
type(normal_spin) nf_S
TYPE(spinor_8) N_AXIS
TYPE(res_spinor_8) N_AXIS_res
real(dp) ag,ag0,gamma0 
integer number_of_ac_plane
real(dp) x(6),circ,xt(lnv),prec 
logical(lp) mis
type(work) E_0 
!-----------------------------------

type(c_damap) c_map,c_spin0,a_cs,a0,a1,a2,r,as
type(c_normal_form) c_n
TYPE(c_spinor) C_N0
type(c_taylor) phase(3),tune
    interface
       subroutine build_ALS(ALS,MIS)
         use madx_ptc_module
         use pointer_lattice
         implicit none
         type(layout), target :: ALS
         logical(lp) mis
       end subroutine build_ALS
    end interface



Lmax = 10.d0
use_info = .true.
clockwise=1 !  Real FPP spin tune definition 
spin_tune_def=clockwise  ! Complex and Real FPP have same tune definition 
prec=1.d-10 !


call ptc_ini

ALS=>m_u%start

Write(6,*) " small misalignments and rotations in lattice ? input t or f "
read(5,*) mis

call build_ALS(ALS,mis) 

 
m_U%start%name="ALS "
write(6,*) "Making an node layout"
CALL MAKE_NODE_LAYOUT(m_U%start)

als=>m_U%start
p=>als%start


!!!! circ is the circumference of the ring !!!! 
call get_length(als,circ)

!!!! modulate.txt sets the magnet BEND1 as a modulated magnet !!!! 
 call kanalnummer(mf,file="AC_modulation.txt")
  write(mf,*) "select layout"
  write(mf,*) 1
  write(mf,*) " MODULATE"
  write(mf,*) " BEND1" 
  write(mf,*) "1.d0 1.d-3 0.00d0   !   DC_ac,A_ac,theta_ac"
  write(mf,*) "1.d0   1      ! 0  !D_ac,n_ac, n_coeff "
  write(mf,*) "1 0 0.000003d0 "
  write(mf,*) "0  0 0 "
  write(mf,*) " return "
 close(mf)
 call read_ptc_command77("AC_modulation.txt")

!

ag0=  1.158630183290000D-003*1.001
p=>als%start
E_0=p
 
!!!!!  Set all magnets spin to g=ag0 to fake a resonance!!!!!! 
p=>als%start
do i=1,als%n
 p%ag = ag0  
 IF(P%MAG%P%NMUL>=3) THEN
  CALL ADD(P,3,0,0.D0) ! making the lattice linear (not really needed)
 ENDIF
 p=>p%next
enddo
 
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

closed=0.d0
state=default0+nocavity0  

    CALL FIND_ORBIT(ALS,CLOSED,1,STATE,c_1d_5)
 
call kanalnummer(mf,"spin_result.txt")

state=default0+only_4d0+SPIN0
XS0=CLOSED
    CALL FIND_ORBIT_probe_spin(ALS,XS0,STATE,c_1d_5,FIBRE1=1,theta0=theta0)

write(mf,*) "Closed Orbit "
WRITE(mf,*) XS0%X
write(mf,*) " Spin Tune in PTC (Real Package definition) "
WRITE(mf,*) THETA0/twopi
N0=XS0%S(1)%X
write(mf,*) " n0 from PTC "
write(mf,*) n0

state=default0+only_4d0+SPIN0+modulation0

!!!! set a modulation clock !!!!!!
xs0%ac%om=0.401073997029287d0*twopi/circ
xs0%ac%x(1)=1.d0
xs0%ac%x(2)=0.d0
write(6,*) " Modulation tune =",circ*xs0%ac%om/twopi

!!!! xs0%ac%x(1) and xs0%ac%x(2) will be the (q,p) of the modulating clock
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

!number_of_ac_plane=1
!    CALL INIT(STATE,2,0)
!call c_init(c_%NO,c_%nd,c_%np,c_%ndpt,number_of_ac_plane,ptc=my_true)  
call init_all(state,3,0)

call alloc(c_map,a_cs,a0,a1,a2,r,as)
call alloc(c_n)
call alloc(c_spin0)
CALL ALLOC(C_N0)
CALL ALLOC(phase)
CALL ALLOC(tune)
 

    call alloc(xs)
    call alloc(theta)
    call alloc(N_AXIS_res)
    call alloc(N_AXIS)
    call alloc(id_s,a_S,A_f,a_l,a_nl,dr)
    call alloc(nf_S)

!!!! Polymorphic probe is created in the usual manner   
   ID_S=1
   
   XS=XS0+ID_S     !
   
!!!! get spin polymorphic probe after one turn   
CALL TRACK_PROBE(ALS,XS,STATE,FIBRE1=1)
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 

!!!! Copy probe_8 into a damapspin
ID_S=XS
!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! 
 
c_map=id_s

c_n%nres=1
c_n%m(3,1)=1
c_n%ms(1)=spin_tune_def
call c_normal(c_map,c_n,dospin=my_true)

 c_n%a_t=c_n%as*c_n%a_t
call c_full_canonize(c_n%a_t,a_cs,as,a0,a1,a2,r,phase) 

 
write(6,*) c_n%tune(1:c_%nd)
write(6,*) " spin Complex",c_n%spin_tune
 
!c_n%As=1
!c_n%a_t=a_cs
 
 !a_cs= c_n%a_t
!a_cs=as*a0*a1*a2

!c_map=c_n%A_t**(-1)*c_n%As**(-1)*c_map*c_n%As*c_n%A_t
c_map=a_cs**(-1)*c_map*a_cs

 
c_map=to_phasor()*c_map*from_phasor()
 

c_spin0=1
C_SPIN0%S=c_map%S.SUB.0
C_SPIN0=C_SPIN0**(-1)*C_MAP
 
          call c_find_om_da(c_spin0%s,C_N0)
          call c_n0_to_nr(C_N0,C_N0)
write(mf,*) "   "
write(mf,*) " Complex FPP results "
write(mf,*) " Exponent of Factorized Axis in Resonance Basis "
CALL PRINT(C_N0,mf,PREC)
 

!!!!!!!!!!  Find H_  !!!!!!!!!!!!!!!!!!!!!!
! normal form with one resonance
!integer M(NDIM,NRESO),MS(NRESO),NRES  ! orbital and spin resonances to be left in the map
 
NF_S%M(3,1)=1     !!!!  nu3+ns=integer(=0 here). nu3 is the spin of modulation.
NF_S%MS(1)=clockwise
NF_S%nres=c_n%nres

!!! Map normalised
NF_S=ID_S
write(6,*) "nx,ny,nu_modulation,nu_spin"
write(6,'(4(1x,E15.7))') NF_S%n%tune(1:c_%nd),NF_S%nu

call factor(NF_S%a_t,a_s,a_f,a_l,a_nl,dr=dr)

!NF_S%a_t=a_s*a_f*a_l*a_nl  not necessary because factor updates input, not true of c_full_canonize

ID_S=NF_S%a_t**(-1)*ID_S*NF_S%a_t     !!!!  this new map should have only one resonance left nu3+ns=0
 
A_f=ID_S
a_f%m=1
CALL clean_orbital_33(A_f%S%S,A_f%S%S)  !  The map a_f is now only a pure x-independent spin matrix a_f=(I,S_0) S_0=exp(theta_0 L_y)

A_S=id_s
a_s%m=1
a_f=a_f**(-1)*a_s     !!!   a_S= S_0 A_f    so a_f is a spin matrix with NO constant part. 
CALL dalog_spinor_8(a_f,N_AXIS)   !!!  a_f=exp(N_AXIS dot L)  ("DA" calculation: only works on spin matrix with no constant part)

N_AXIS_res=N_AXIS

call clean_taylor(N_AXIS_res%X(1)%T%R,N_AXIS_res%X(1)%T%R,prec=prec)
call clean_taylor(N_AXIS_res%X(1)%T%I,N_AXIS_res%X(1)%T%I,prec=prec)
call clean_taylor(N_AXIS_res%X(2)%T%r,N_AXIS_res%X(2)%T%r,prec=prec)
call clean_taylor(N_AXIS_res%X(2)%T%i,N_AXIS_res%X(2)%T%i,prec=prec)
call clean_taylor(N_AXIS_res%X(3)%T%r,N_AXIS_res%X(3)%T%r,prec=prec)
call clean_taylor(N_AXIS_res%X(3)%T%i,N_AXIS_res%X(3)%T%i,prec=prec)
call clean_taylor(NF_S%n%dhdj%v(1),NF_S%n%dhdj%v(1),prec=prec)
call clean_taylor(NF_S%n%dhdj%v(2),NF_S%n%dhdj%v(2),prec=prec)
call clean_taylor(NF_S%n%dhdj%v(3),NF_S%n%dhdj%v(3),prec=prec)
write(mf,*) "   "
write(mf,*) "*************************************************************************"
write(mf,*) "                  Comparing results (used different A)                   "
write(mf,*) "Norm of coefficient c_n0%v(1)=       ", abs(C_N0%v(1).sub.'000010')            
write(mf,*) "Norm of coefficient N_AXIS_res%x(1)= ", abs(N_AXIS_res%x(1).sub.'000010')
write(mf,*) "*************************************************************************"

write(mf,*) "   "
write(mf,*) " Real FPP results "
write(mf,*) " Exponent of Factorized Axis in Resonance Basis "

call print(N_AXIS_res,mf)


 close(mf)



    call KILL(id_S,A_S,A_f)
    call KILL(nf_S)
    call KILL(xs)
    call KILL(theta)
    call KILL(N_AXIS_res)

 call ptc_end


end program ptc_geometry

subroutine  build_ALS(ALS,mis)
use madx_ptc_module
use pointer_lattice
implicit none

type(layout), target :: ALS

real(dp) :: alpha,lbend, cut, ksd, ksf 
type(fibre)  L1,L2,L3,L4,L5,L6,L7,L8,L9,L10
type(fibre)  L11,L12,L13,L14,L15,L16,L17,L18,L19,L20
type(fibre)  L21,L22,L23,L24,L25,L26,L27,L27A,L27B,L27C,L27D,DS
 type(fibre)  QF1,QF2,QD1,QD2,QFA1,QFA2,sf,sd,cav,bend,vc5,bend1
type(layout) :: sfline,sdline,sup1,supb
logical(lp) mis
!-----------------------------------

call make_states(.true.)
exact_model = .false.
!default = default + nocavity  
call update_states
madlength = .false.


call set_mad(energy = 1.5d0, method = 6, step = 3)

madkind2 = drift_kick_drift


  L1  = drift("L1 ",  2.832695d0)
  L2  = drift("L2 ",  0.45698d0)
  L3  = drift("L3 ",  0.08902d0)
  L4  = drift("L4 ",  0.2155d0)
  L5  = drift("L5 ",  0.219d0)
  L6  = drift("L6 ",  0.107078d0)
  L7  = drift("L7 ",  0.105716d0)
  L8  = drift("L8 ",  0.135904d0)
  L9  = drift("L9 ",  0.2156993d0)
  L10 = drift("L10",  0.089084d0)
   L11= drift("L11",  0.235416d0)
   L12= drift("L12",  0.1245d0)
   L13= drift("L13",  0.511844d0)
   L14= drift("L14",  0.1788541d0)
   L15= drift("L15",  0.1788483d0)
   L16= drift("L16",  0.511849d0)
   L17= drift("L17",  0.1245d0)
   L18= drift("L18",  0.235405d0)
   L19= drift("L19",  0.089095d0)
   L20= drift("L20",  0.2157007d0)
   L21= drift("L21",  0.177716d0)
   L22= drift("L22",  0.170981d0)
   L23= drift("L23",  0.218997d0)
 L24 = drift ("L24",  0.215503d0)
 L25 = drift ("L25",  0.0890187d0)
 L26 = drift ("L26",  0.45698d0)
 L27 = drift ("L27",  2.832696d0)
 L27a  = drift (" L27a",  0.8596d0)
 L27b  = drift (" L27b",  0.1524d0)
 L27c  = drift (" L27c",  0.04445d0)
 L27d  = drift (" L27d",  1.776246d0)
 ds  = drift (" DS  ", 0.1015d0)

  QF1 = QUADRUPOLE(" QF1 ",0.344D0, K1= 2.2474D0+6.447435260914397D-03)
  QF2 = QUADRUPOLE(" QF2 ",0.344D0, K1= 2.2474D0)
  QD1 = QUADRUPOLE(" QD1 ",0.187D0, K1= -2.3368D0-2.593018157427161D-02); 
  QD2 = QUADRUPOLE(" QD2 ",0.187D0, K1= -2.3368D0);  
  QFA1= QUADRUPOLE(" QFA1",0.448D0, K1= 2.8856D0);  
  QFA2= QUADRUPOLE(" QFA2",0.448D0, K1= 2.8856D0);  

!!! 1/2 mad-x value
ksf=(-41.67478927130080d0+0.3392376315938252d0);ksd= (56.36083889436033d0-0.1043679358857811d0);
   sf=sextupole ("sf",2.d0*0.1015d0, K2= ksf);
   sd= sextupole("sd", 2.d0*0.1015d0, K2= ksd);

 VC5=marker("vc5");
ALPHA=0.17453292519943295769236907684886d0;
 
LBEND=0.86621d0;
 
 
BEND = RBEND("BEND", LBEND, ANGLE=ALPHA).q.(-0.778741d0)
BEND1 = RBEND("BEND1", LBEND, ANGLE=ALPHA).q.(-0.778741d0)
 
CAV=RFCAVITY("CAV",L=0.0000d0,VOLT=-1.0d0,REV_FREQ=500.0d6)


sfline=1*sf;
sdline=1*sd;

SUP1=L1+L2+L3+QF1+VC5+L4+L5+QD1+L6+L7+L8+VC5+BEND+VC5+L9+sfline+L10+&
           L11+QFA1+L12+sdline+L13+ &
           L14+BEND+L15+L16+sdline+L17+ &
           QFA2+L18+L19+sfline+L20+BEND+L21+&
           L22+QD2+L23+L24+QF2+L25+ &
           L26+VC5+L27;

SUPb=L1+L2+L3+QF1+VC5+L4+L5+QD1+L6+L7+L8+VC5+BEND+VC5+L9+sfline+L10+&
           L11+QFA1+L12+sdline+L13+ &
           L14+BEND+L15+L16+sdline+L17+ &
           QFA2+L18+L19+sfline+L20+BEND1+L21+&
           L22+QD2+L23+L24+QF2+L25+ &
           L26+VC5+L27;

ALS = 11*sup1+supb+cav;
 
ALS = .ring.ALS

call survey(ALS)


if(mis) then
 sig=1.d-5
 cut=4.d0
 call MESS_UP_ALIGNMENT(ALS,SIG,cut)
endif
end subroutine build_ALS